/*
 * rosa_semaphores.c
 *
 * Created: 2014-12-03 13:33:13
 *  Author: Andreas Domfors
 */ 

#include <stdlib.h>
#include <stdbool.h>
#include <malloc.h>
#include "kernel/rosa_systick.h"
#include "kernel/rosa_task.h"
#include "kernel/rosa_scheduler.h"
#include "kernel/rosa_queues.h"
#include "kernel/rosa_util.h"

#include "API/rosa.h"
#include "kernel/rosa_semaphore_internal.h"

/// A linked list of all semaphores
SemaphoreImpl *semaphoreListHead = NULL;

/// Each semaphore has a list of tasks that are waiting for the semaphore to 
/// be available. This function adds a new task to that list. The list is sorted
/// on dynamic priority. While blocked, a task's dynamicPriority never changes.
static void addTaskToSemaphoreBlockedList(SemaphoreImpl *s, tcb *newTask)
{
	tcb *prev, *iterator = s->blockListHead;
	if(s->blockListHead == NULL || newTask->dynamicPriority > s->blockListHead->dynamicPriority)
	{
		s->blockListHead = newTask;
		newTask->nextInBlockList = iterator;
	}
	else
	{
		while (1) 
		{
			if(iterator->nextInBlockList == NULL)
			{
				iterator->nextInBlockList = newTask;
				newTask->nextInBlockList = NULL;
				break;
			}
			if(newTask->dynamicPriority > iterator->dynamicPriority)
			{
				prev->nextInBlockList = newTask;
				newTask->nextInBlockList = iterator;
			}
			prev = iterator;
			iterator = iterator->nextInBlockList;			
		}
	}
}

/// Each semaphore has a list of tasks that are waiting for the semaphore to
/// be available. This function removes a specified task from that list.
static void removeTaskFromSemaphoreBlockedList(SemaphoreImpl *sem, tcb *task)
{
	if(sem->blockListHead == task)
	{
		sem->blockListHead = task->nextInBlockList;
		task->nextInBlockList = NULL;
	}
	else
	{
		for(tcb *iterator = sem->blockListHead; iterator->nextInBlockList != NULL; iterator = iterator->nextInBlockList)
		{
			if(iterator->nextInBlockList == task)
			{
				iterator->nextInBlockList = task->nextInBlockList;
				task->nextInBlockList = NULL;
				return;
			}			
		}
		breakpoint;		// Error: Couldn't find task in blocklist
	}
}

/// This function gives the specified semaphore to the highest-priority
/// task that is waiting for it. The task is woken up and placed in the
/// ready queue. Returns a pointer to the woken task.
static tcb *giveSemaphoreToHighestPriorityBlockedTask(SemaphoreImpl *sem)
{
	tcb *wokenTask = sem->blockListHead;
	removeTaskFromSemaphoreBlockedList(sem, wokenTask);
	if(wokenTask->semStatus == SOS_InfiniteWait)
		rosa_removeFromQueue(wokenTask, &blockedQueueHead);
	else if(wokenTask->semStatus == SOS_FiniteWait)
		rosa_removeFromQueue(wokenTask, &waitingQueueHead);
	else
		assert(false);
		
	wokenTask->state = Ready;
	wokenTask->semStatus = SOS_GotSemaphore;
	rosa_addToReadyQueue(wokenTask);
	
	return wokenTask;
}

/// Times out the semaphore operation that the specified task is performing
/// The task is woken up and placed in the ready queue
void timeoutSemaphoreOperation(struct tcb_record_t *task)
{
	removeTaskFromSemaphoreBlockedList(task->semaphoreThatWereWaitingFor, task);
	task->semStatus = SOS_Timeout;	
}

static void setTaskToBlockedState(SemaphoreImpl *sem, tcb *task)
{
	task->state = Blocked;
	addTaskToSemaphoreBlockedList(sem, task);
	task->semaphoreThatWereWaitingFor = sem;
}

/// There's a linked list of all semaphores. This function adds a new 
/// semaphore to that list
static void addSemaphoreToSemaphoreList(SemaphoreImpl *new)
{
	new->next = semaphoreListHead;
	semaphoreListHead = new;
}

/// Creates and initializes a binary semaphore at the specified
/// address. The semaphore is available when created.
int ROSA_createBinarySemaphore(semaphore *s)
{
	SemaphoreImpl *sem;
	*s = malloc(sizeof(SemaphoreImpl));
	if(*s == NULL)
		return -1;
	
	sem = (SemaphoreImpl *)(*s);
	sem->type = TYPE_BINARY;
	sem->next = NULL;
	sem->blockListHead = NULL;
	sem->lockListNext = NULL;
	sem->ceiling = 0;
	sem->semaphoreIsSignalled = true;
	
	addSemaphoreToSemaphoreList(sem);
	return 0;
}

/* If the semaphore is available, then take it and return. Otherwise put 
the current task in the blocked list (possibly with a timeout), and then 
call the scheduler to do a context switch. */
int ROSA_waitBinarySemaphore(semaphore *s, unsigned int timeout)
{
	SemaphoreImpl *sem = *(SemaphoreImpl **)s;
	if(sem->type != TYPE_BINARY)
	{
		breakpoint;
		return -1;
	}
		
	stopSysTick();
	tcb *currentTask = getCurrentTask();
	if(sem->semaphoreIsSignalled)
	{
		sem->semaphoreIsSignalled = false;
		startSysTick();
		return 0;
	}
	else			// Semaphore is not available. Block.
	{
		setTaskToBlockedState(sem, currentTask);
		
		if(timeout == 0)
		{
			currentTask->semStatus = SOS_InfiniteWait;
		}
		else
		{
			currentTask->semStatus = SOS_FiniteWait;
			currentTask->wakeUpTime = ROSA_getCurrentTick() + timeout;
		}
		scheduler();			// Switch to another task. Scheduler re-starts systick.
		switch(currentTask->semStatus)
		{
			case SOS_GotSemaphore:
				return 0;
			case SOS_Timeout:
				return -2;
			default:
				assert(false);
				return -1;
		}
	}
}

/// Signals the specified binary semaphore.
int ROSA_signalBinarySemaphore(semaphore *s)
{
	SemaphoreImpl *sem = *(SemaphoreImpl **)s;
	if(sem->type != TYPE_BINARY)
	{
		breakpoint;
		return -1;
	}
	
	stopSysTick();
	if(sem->blockListHead != NULL)					// If there are tasks waiting for this semaphore
	{
		giveSemaphoreToHighestPriorityBlockedTask(sem);
		scheduler();	// Possibly switch task. Scheduler re-starts systick.
	}
	else											// If there aren't any tasks waiting for this semaphore
	{
		sem->semaphoreIsSignalled = true;			// Mark it as available and continue
	}
	return 0;
}

/// Creates and initializes an IPCP semaphore at the location s.
/// The semaphore is available when created.
int ROSA_createSemaphoreIPCP(IPCPsemaphore *s, unsigned int ceiling)
{
	SemaphoreImpl *sem;

	*s = malloc(sizeof(SemaphoreImpl));
	if(*s == NULL)
		return -1;
	
	sem = (SemaphoreImpl *)(*s);
	sem->next = NULL;
	sem->type = TYPE_IPCP;
	sem->blockListHead = NULL;
	sem->lockListNext = NULL;
	sem->semaphoreIsSignalled = true;
	sem->ceiling = ceiling;
	
	addSemaphoreToSemaphoreList(sem);
	return 0;
}

/// Each task has a list of locked IPCP semaphores. This function adds a semaphore
/// to that list
static void addSemaphoreToTaskLockList(tcb *task, SemaphoreImpl *sem)
{
	sem->lockListNext = task->lockedSemaphoresListHead;
	task->lockedSemaphoresListHead = sem;
}

/// Each task has a list of locked IPCP semaphores. This function adds a semaphore 
/// to that list
static void removeSemaphoreFromTaskLockList(tcb *task, SemaphoreImpl *sem)
{
	assert(task->lockedSemaphoresListHead);
	if(task->lockedSemaphoresListHead == sem)
	{
		task->lockedSemaphoresListHead = task->lockedSemaphoresListHead->lockListNext;
	}
	else
	{
		SemaphoreImpl *iterator = task->lockedSemaphoresListHead;
		while(iterator->lockListNext != NULL)
		{
			if(iterator->lockListNext == sem)
			{
				iterator->lockListNext = sem->lockListNext;
			}
			iterator = iterator->lockListNext;
		}
	}
}

static bool taskHasLockedSemaphore(tcb *task, SemaphoreImpl *sem)
{
	SemaphoreImpl *iterator = task->lockedSemaphoresListHead;
	while(iterator)
	{
		if(iterator == sem)
			return true;
		iterator = iterator->lockListNext;
	}
	return false;
}

/// Returns true if the task has locked any IPCP semaphore
bool taskHasLockedIPCPsemaphore(struct tcb_record_t *task)
{
	return (task->lockedSemaphoresListHead != NULL);
}

void recalculateTaskDynamicPriority(tcb *task)
{
	task->dynamicPriority = task->priority;
	SemaphoreImpl *iterator = task->lockedSemaphoresListHead;
	while(iterator != NULL) {
		if(task->dynamicPriority < iterator->ceiling)
			task->dynamicPriority = iterator->ceiling;
		iterator = iterator->lockListNext;
	}
}

/// Waits for the specified IPCP semaphore. If (timeout) ticks
/// pass without the semaphore becoming available, the operation
/// times out.
int ROSA_waitSemaphoreIPCP(IPCPsemaphore *s, unsigned int timeout)
{
	SemaphoreImpl *sem = (SemaphoreImpl *)*s;
	if(sem->type != TYPE_IPCP)
	{
		breakpoint;
		return -1;
	}
	tcb *currentTask = getCurrentTask();
	if (sem->ceiling < currentTask->priority)
	{
		return -1;	// Error: Ceiling is too low
	}
	
	stopSysTick();
	if(taskHasLockedSemaphore(currentTask, sem))	// Check if task has already locked semaphore - we don't handle recursive locking
	{
		startSysTick();
		return -1;		// Error: this task has already locked the semaphore
	}
	if(sem->semaphoreIsSignalled)
	{
		sem->semaphoreIsSignalled = false;
		addSemaphoreToTaskLockList(currentTask, sem);
		recalculateTaskDynamicPriority(currentTask);
		startSysTick();
		return 0;
	}
	else			// Semaphore is not available. Block.
	{
		setTaskToBlockedState(sem, currentTask);
		if(timeout == 0)
		{
			currentTask->semStatus = SOS_InfiniteWait;
		}
		else
		{
			currentTask->semStatus = SOS_FiniteWait;
			currentTask->wakeUpTime = ROSA_getCurrentTick() + timeout;
		}
		
		scheduler();		// Switch to another task. Scheduler re-starts systick
		
		// Execution resumes here when the operation completed or had a timeout
		switch(currentTask->semStatus)
		{
			case SOS_GotSemaphore:
				return 0;
			case SOS_Timeout:
				return -2;
			default:
				assert(false);
				return -1;
		}
	}	
}

/// Signals the specified IPCP semaphore. The task that calls this must
/// be the last task that successfully waited for the semaphore.
int ROSA_signalSemaphoreIPCP(IPCPsemaphore *s)
{
	SemaphoreImpl *sem = (SemaphoreImpl *)*s;	
	if(sem->type != TYPE_IPCP)
	{
		breakpoint;
		return -1;
	}
	tcb *currentTask = getCurrentTask();
	if (sem->ceiling < currentTask->priority)
	{
		return -1;	// Error: Ceiling is too low
	}
	
	stopSysTick();
	if(!taskHasLockedSemaphore(currentTask, sem))	// Check that task has locked semaphore
	{
		startSysTick();
		return -1;
	}
	if(sem->blockListHead != NULL)					// If there are tasks waiting for this semaphore
	{
		tcb *wokenTask = giveSemaphoreToHighestPriorityBlockedTask(sem);
		
		removeSemaphoreFromTaskLockList(currentTask, sem);
		addSemaphoreToTaskLockList(wokenTask, sem);
		
		recalculateTaskDynamicPriority(currentTask);
		recalculateTaskDynamicPriority(wokenTask);
		scheduler();	// Possibly switch to the woken task. Scheduler re-starts systick.
	}
	else											// If there aren't any tasks waiting for this semaphore
	{
		sem->semaphoreIsSignalled = true;			// Mark it as available and continue
		removeSemaphoreFromTaskLockList(currentTask, sem);
		recalculateTaskDynamicPriority(currentTask);
		scheduler();								// Call the scheduler if necessary. Scheduler re-starts systick.
	}
	return 0;
}
